#include <avr/wdt.h>
#include <PID_v1.h>
#include <Metro.h>

const byte PWMPin = 3; //Analog
const byte HoldPin = 6; //Analog
const int HoldMax = 200; //Max EinstellWert
const int HoldMin = 20; //Min EinstellWert
const int SensMax = 2000; //Max Sensorwert
const int SensMin = 0; //Min Sensorwert
const int SensSamples = 20; //Anzahl Messungen (Sensor smooth)

float Setpoint, Input, Output;

int SensValArray[SensSamples]; //SensorSmooth
int aktPWMVal=0; //aktueller PWMwert

Metro timerUpdateOutput(1000);
Metro timerUpdateSerial(1000);
Metro timerUpdatePWMOutput(100);
Metro timerReadSens(200);

void setup() {
  pinMode(13, OUTPUT);
  digitalWrite(13,HIGH);
  pinMode(PWMPin, OUTPUT);
  digitalWrite(PWMPin,LOW); //Lampe an

  Serial.begin(19200);
  setupSensor();

  //Input
  Input = BH1750_Read();
  for(int i=0;i<=SensSamples;i++)
    SensValArray[i]=Input;
  //Init von digitalSmooth (wegen static Variablen)
  for(int i=0;i<SensSamples;i++)
    digitalSmooth(Input, SensValArray);

  //SetPoint (PWMVal)   
  int SensVal = analogRead(HoldPin);
  SensVal = map(SensVal, 0, 1024, HoldMin, HoldMax);
  Setpoint = SensVal;

  if((Input-Setpoint)>10){ //Wenn Input viel größer als SetPoint dann Lampe aus
    aktPWMVal = 255;
    digitalWrite(PWMPin,HIGH);
  }
  Output = aktPWMVal;
  
  //Start Watchdog
  wdt_enable (WDTO_8S);  // reset after one second, if no "pat the dog" received
  digitalWrite(13,LOW);
}

double oldHash = -1; //Hash der Ausgabewerte um doppelte Einträge zu vermeiden
unsigned long ZeitPunktAus=0;

void loop() { 
  wdt_reset ();  // give me another second to do stuff (pat the dog)
  
  if (timerReadSens.check()) {
    //SetPoint (PWMVal)   
    int SensVal = analogRead(HoldPin);
    SensVal = map(SensVal, 0, 1024, HoldMin, HoldMax);
    if(abs(SensVal-Setpoint)>1) //kleinere Sprünge ignorieren
      Setpoint = SensVal;
      
    //Input (Lichtsensor)
    SensVal = BH1750_Read(); 
    SensVal = digitalSmooth(SensVal, SensValArray);
    Input = SensVal; 
  }

  if (timerUpdateOutput.check()) {
    float abweichung = calcProzent(Input, Setpoint);
    if((aktPWMVal<255) || (abweichung>10.0)) {  //Bei abgeschalteter Lampe erst wieder Regeln, wenn abweichung > 10.0% ist
      float error = Input - Setpoint;
      error = constrain(error, -20, 20);
      Output += error;
      Output = constrain(Output, 0, 255);
    }
  }

  if (timerUpdateSerial.check()) {
    double Hash=Input+Setpoint+Output+aktPWMVal;
    if (Hash!=oldHash){ //Werte haben sich verändert
      oldHash=Hash;
      Serial.print(Input,0); Serial.print(" Input\t"); 
      Serial.print(Setpoint,0); Serial.print(" Setpoint\t");
      Serial.print(Output,0); Serial.print(" Output\t");
      Serial.print(aktPWMVal); Serial.print(" aktPWMVal");
      Serial.println("");Serial.flush();
    }
  }

  //PWMWert schreiben
  if (timerUpdatePWMOutput.check()) {
    boolean regelungAktiv = (abs(aktPWMVal-Output)>0); //mindest Abweichung 1
    if (aktPWMVal>220){
      regelungAktiv=(Output<220);
      if(!regelungAktiv){
        if(ZeitPunktAus==0)
          ZeitPunktAus = millis();
        
        //Lampe aus wenn WarteZeit abgelaufen und Abweichung von Input zu Setpoint > 10%
        if( ((millis() - ZeitPunktAus)>600000) && (calcProzent(Input, Setpoint)>30.0) ) 
          aktPWMVal=255;
      }
    }

    if (regelungAktiv){
      ZeitPunktAus=0;
      if(int(Output)>aktPWMVal){
        float error = float(aktPWMVal)/float(Output);
        error = 1-error;
        if(error > 0.5){
          aktPWMVal+=2;
        }
        else{
          aktPWMVal+=1;
        }
        if (aktPWMVal>255)
          aktPWMVal=255;   
      }
      if(int(Output)<aktPWMVal){
        float error = float(Output)/float(aktPWMVal);
        error = 1-error;
        if(error > 0.5){
          aktPWMVal-=2;
        }
        else {
          aktPWMVal-=1;
        }
        if (aktPWMVal<0)
          aktPWMVal=0;
      }
    } 
    analogWrite(PWMPin,aktPWMVal);
  }

  delay(1);
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  unsigned long total;
  static int i;
  static int sorted[SensSamples];
  boolean done;

  i = (i + 1) % SensSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<SensSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (SensSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((SensSamples * 15)  / 100), 1); 
  top = min((((SensSamples * 85) / 100) + 1  ), (SensSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}

float calcProzent(float a, float b){  //prozentuale Abweichung zweier Werte
  if(a>b){
    return (100.0-(b/a)*100.0);
  }
  else {
    return (100.0-(a/b)*100.0);
  }
}

